/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
* This is an effort to test if the Occupancygrid can be used for SLAMing
*/ 

#include<stdio.h>
#include<stdlib.h>
#include<math.h>
#include "CScanMatchInterface.h" ///< Scanmatch interface
#include "CAHistogramECorrelationScanMatch.h" ///< Scanmatch 
#include "TOccupancyGrid.h" ///< Occupancy grid class
#include "glutImageGUI.h"   ///< For visualisation
#include "JPEGEncoder.hpp"  ///< For saving JPEG

int readLaserOdometryData(FILE *f,ScanMatch::scan &meas, ScanMatch::pose &p){
	float p1,p2,p3;
	int i;
	char c;
	fscanf(f,"%f %f %f %f %f %f ",&p1,&p2,&p3, &p.x,&p.y,&p.a); ///< Pose
	for(i=0;i<361;i++){
		fscanf(f,"%f",&meas.r[i]);
		meas.a[i]=0.5*i*M_PI/180.0;
		fscanf(f,"%c",&c);
   // fprintf(stderr,"%.1f ",meas.r[i]);
		if(c=='\n' && i<361){
			fprintf(stderr,"EOL reached at i=%d\n",i);
			meas.N = i;
			return 0;
		}
	}
	meas.N = 361;
	return 0;

}


void scan2absoluteXY( ScanMatch::scan &s, float *x, float *y, ScanMatch::pose &pose){
	int i;
	if(x==NULL || y==NULL) return;
	for(i=0;i<s.N;i++){
		x[i]=s.r[i]*cos(s.a[i]+pose.a)+pose.x;
		y[i]=s.r[i]*sin(s.a[i]+pose.a)+pose.y;
	}
}

float minMSD(float *x,float *y, float px,float py, int N, int &mini, float &dx2, float &dy2){
	float dist2=100000;
	float d2;
	int minInd = 0;
	float dx,dy;
	for(int i=0;i<N;i++){
		dx = (x[i]-px)*(x[i]-px);
		dy = (y[i]-py)*(y[i]-py);
		d2 = dx + dy;
		if(d2<dist2){ 
			dist2 = d2;
			dx2 = dx;
			dy2 = dy;
			minInd = i;
		}
	}
	mini = minInd;
	return dist2;
}

float computeMSE(ScanMatch::scan &s1,ScanMatch::scan &s2,ScanMatch::pose &p1,ScanMatch::pose &p2, 
							float &x_err ,float &y_err)
{
	float *x1 = (float *) malloc(s1.N*sizeof(float));  
	float *y1 = (float *) malloc(s1.N*sizeof(float));
	float *x2 = (float *) malloc(s2.N*sizeof(float));  
	float *y2 = (float *) malloc(s2.N*sizeof(float));

	long int ss1,us1,ss2,us2,sec,us,ms;
	ownTime_GetTimeOfDay(&ss1,&us1);

	scan2absoluteXY(s1,x1,y1,p1);
	scan2absoluteXY(s2,x2,y2,p2);
	
	int mini;
	float dist=0,d;
	float dx2=0,dy2=0,ddx,ddy;
	int num=0;
	for(int i=0;i<s1.N;i++){
		if(s1.r[i] <= 0 || s2.r[i]<=0) continue;
		d = minMSD(x2,y2,x1[i],y1[i],s2.N,mini,ddx,ddy);
		if(d<1.0){
			dist += d; 
			dx2 += ddx;
			dy2 += ddy;
			num++;
		}
	}	

	ownTime_GetTimeOfDay(&ss2,&us2);
	sec = ss2-ss1;us = us2-us1;ms = sec*1000+us/1000;
	float fms=(float)sec*1000.0+(float)us/1000.0;
	//fprintf(stderr,"It took %.4f ms to calculate\n",fms);
	
	//fprintf(stderr,"MSE= %.2f MSE(X)=%.2f MSE(Y)=%.2f", dist/num,dx2/num,dy2/num);
	//fprintf(stderr," ME= %.2f ME(X)=%.2f ME(Y)=%.2f\n", sqrt(dist/num),sqrt(dx2/num),sqrt(dy2/num));
	//x_err = sqrt(dx2/num);
	//y_err = sqrt(dy2/num);
	if(num!=0){
		x_err = sqrt(dx2/num);
		y_err = sqrt(dy2/num);
	}
	else{
		x_err = -666;
		y_err = -666;
	}
	free(x1);
	free(x2);
	free(y1);
	free(y2);
	
	if(num!=0){ 
		return dist/num;
	}
	return -666; 
}

int filterScan(ScanMatch::scan &m, Grid::TScan &g){
		int bad=0;
		float dist_th=1.0;
		float dr_plus,dr_minus;
		
		for(int i=1;i<m.N-1;i++){
				dr_plus = fabs(m.r[i]-m.r[i+1]);
				dr_minus = fabs(m.r[i]-m.r[i-1]);
				if(dr_plus>dist_th && dr_minus>dist_th) g.r[i] = -1.0; 
		}
		for(int i=0;i<m.N;i++){
				g.a[i] = m.a[i];
				if(g.r[i] == -1){ g.r[i]=0; bad++;} 
				else g.r[i] = m.r[i];
				if(g.r[i] > 35){g.r[i] = 0; bad++;}///Filter out long distances
		}
		return bad;
}

void setRGBpix(unsigned char *rgb,int i,int j, int Nx, unsigned char r,unsigned char g, unsigned char b ){
	rgb[3*Nx*j+3*i]=r;
	rgb[3*Nx*j+3*i+1]=g;
	rgb[3*Nx*j+3*i+2]=b;
}

void setRGBpixBold(unsigned char *rgb,int i,int j, int Nx, unsigned char r,unsigned char g, unsigned char b){
	for(int ii=i-1;ii<i+1;ii++){
		for(int jj=j-1;jj<j+1;jj++){
			setRGBpix(rgb,ii,jj,Nx,r,g,b);
		}
	}

}


void saveJPEG(const char *fname, unsigned char *rgb,int Nx, int Ny){
	CJPEGEncoder encoder;
	encoder.SetRGBDataPtr((const char *)rgb,Nx,Ny);
	FILE *f = fopen(fname,"wb");
	fwrite(encoder.GetJPEGDataPtr(), 
				 encoder.GetJPEGDataSize(), 
																 1, f);
	fclose(f);
		
}


int main(int argc, char *argv[]){
		TOccupancyGrid O(150.1,150.1,0.1,Grid::TCoord2D(0,0));
		
		int Nx,Ny;
		unsigned char *rgb;
		
		
		if(argc != 2){
			fprintf(stderr,"\nUsage: ./laser_odo [dir]\n\n");
			fprintf(stderr,"[dir] - the directory where the laser_update.txt is located\n");
			return 0;
		}
		char *directory=argv[1];
		
		fprintf(stderr,"Using directory %s\n",directory);
		
		if(chdir(directory)==-1){
			fprintf(stderr,"Invalid directory '%s'!\n",directory);
			return 0;
		}
		
		FILE *log = fopen("laser_update.txt","rt");
		if(log==NULL){
			fprintf(stderr,"No laser update file!\n");
			return 0;
		}
		
		///Scanmatch stuff
		CScanMatchInterface *matcher;  ///< This is for differential 
		
		matcher = new CAHistogramECorrelationScanMatch(0.32, ///< Search area in meters 
																										0.5);///< Search angle in radians
		
		ScanMatch::pose u_pos, pos,odo_cur,odo_ref,est,rpos;   ///< The estimated position
		ScanMatch::scan meas,ref;  ///< The Reference scan 
		
		meas.allocate(361);
		ref.allocate(361);
		
		//readScanLine181(log,ref,odo_cur);
		readLaserOdometryData(log,ref,odo_cur);
		//odo_cur.set(0,0,0);
		matcher->setReferenceScan(ref,odo_cur); 
		fprintf(stderr,"%.2f %.2f %.2f\n",odo_cur.x,odo_cur.y,odo_cur.a);
		///Initialize grid as empty
 		O.setGridToValue(0.5);
		O.getDim(Nx,Ny);
		rgb = (unsigned char *) malloc(3*Nx*Ny);
		///Initialize Image display
		addImageWindow(Nx, Ny);
		glutImageGUI_Start();
		
		Grid::TScan s; ///< for occupancy grid
		s.alloc(361);
		
		/*
		readScanLine181(log,meas,odo_cur);
		
		for(int i=0;i<meas.N;i++){
				s.a[i] = meas.a[i];
				s.r[i] = meas.r[i];
				if(s.r[i] > 30) s.r[i]=0;
		}
		
		odo_cur.set(0,0,0);
		O.setScanFillProb(odo_cur.x, odo_cur.y ,odo_cur.a, s,0.01,0.99);
		O.fillRGB(rgb,1.0/255.0);
		displayImage(0, rgb,Ny,Nx);
		usleep(1000*2000);
		*/
		long int ss0,us0,ss1,us1,ss2,us2,sec,us,ms;
		float fms;
		ownTime_GetTimeOfDay(&ss0,&us0);
		int cnt=0;
		odo_ref = odo_cur;
		rpos = odo_cur;
		
		ScanMatch::pose est_pos;
		std::vector<ScanMatch::pose> store;
		
		while(!feof(log)){
				//readScanLine181(log,meas,odo_cur);
				readLaserOdometryData(log,meas,odo_cur);
				ownTime_GetTimeOfDay(&ss1,&us1);
				
				est = est.calculateDifferentialSensorMovement(odo_ref,odo_cur, 0);
				est_pos = rpos.integrateDifferential(est);
				
								
				float corr = matcher->update(meas,est_pos);    ///update
				matcher->getCurrentPose(pos); ///< Get the estimate
				
				est = est.calculateDifferentialSensorMovement(pos,est_pos, 0);
				est.topi();
				//fprintf(stderr,"%.2f %.2f %.2f ",est.x,est.y,est.a*180.0/M_PI);
				
				if(fabs(est.a)>1.5*M_PI/180.0) pos.a = est_pos.a;
				//if(corr > 0.3) pos.a = odo_cur.a;
				//else fprintf(stderr,"Ny ");
				store.push_back(pos);
				
				est=est.calculateDifferentialSensorMovement(rpos,pos, 0);
				
				if( (est.x*est.x+est.y*est.y)>1.2*1.2 || fabs(est.a)>65.0*M_PI/180.0 ){
					matcher->setReferenceScan(meas,pos);
					odo_ref = odo_cur;
					rpos = pos;					
					//fprintf(stderr,"ref \n");
				} else
					//fprintf(stderr,"\n");

				matcher->setCurrentPose(rpos);
				for(int i=0;i<meas.N;i++){
						s.a[i] = meas.a[i];
						s.r[i] = meas.r[i];
						if(s.r[i] > 20) s.r[i]=0;
				}
				if(cnt%5==0)O.setScanFillProb(pos.x, pos.y ,pos.a, s,0.4,0.99);
				
				//if(cnt%150==0) {
				//		O.fillRGB(rgb,1.0/255.0);
				//		displayImage(0, rgb,Ny,Nx);
				//}
				cnt++;
		}
		
		ownTime_GetTimeOfDay(&ss2,&us2);
		sec = ss2-ss0;us = us2-us0; ms = sec*1000+us/1000;
		fms=(float)sec*1000.0+(float)us/1000.0;
		fprintf(stderr,"TOTAL::(%.4f ms) = (%.4f)/scan \n",fms,fms/cnt);
		
		FILE *f = fopen("sepi_lodo.txt","wt");
		for(int i=0;i<store.size();i++){
			fprintf(f,"%f %f %f\n",store[i].x,store[i].y,store[i].a);
		}
		fclose(f);
			
		O.fillRGB(rgb,1.0/255.0);
			
		Grid::TCoord2D ocoord;
		Grid::TPixel pix;
			
		for(int i=0;i<store.size();i+=1){
			ocoord.x = store[i].x; ocoord.y = store[i].y;
			pix = O.toPixel(ocoord);
			setRGBpixBold(rgb,pix.x,pix.y, Nx, 255,0, 0 );
		}
		
		saveJPEG("sepi_lodo.jpg",rgb,Nx,Ny);
		
		
		while(1){
			O.fillRGB(rgb,1.0/255.0);
			
			Grid::TCoord2D ocoord;
			Grid::TPixel pix;
			
			for(int i=0;i<store.size();i+=1){
				ocoord.x = store[i].x; ocoord.y = store[i].y;
				pix = O.toPixel(ocoord);
				setRGBpixBold(rgb,pix.x,pix.y, Nx, 255,0, 0 );
			}
			
			displayImage(0, rgb,Ny,Nx);
			usleep(1000*500);
		}
		
		return 0;
}